#include "super_cap.h"

#ifdef USE_FREERTOS
uint8_t super_cap_init(super_cap_TypeDef* hcap, QueueHandle_t can_queue)
{
    if(hcap == NULL)
    {
        return SUPER_CAP_NULL_PTR;
    }
    hcap->can_queue = can_queue;
    return SUPER_CAP_OK;
}
#else
uint8_t super_cap_init(super_cap_TypeDef* hcap, CAN_HandleTypeDef* hcan)
{
    if(hcap == NULL)
    {
        return SUPER_CAP_NULL_PTR;
    }
    hcap->hcan = hcan;
    return SUPER_CAP_OK;
}
#endif

uint8_t super_cap_cmd(super_cap_TypeDef* hcap, float limit_power, float target_power)
{
    uint8_t data[8] = {};
    data[0] = (int16_t)(limit_power * 100.0f) & 0xFF;
    data[1] = (int16_t)(limit_power * 100.0f) >> 8;
    data[2] = (int16_t)(target_power * 100.0f) & 0xFF;
    data[3] = (int16_t)(target_power * 100.0f) >> 8;
#ifdef USE_FREERTOS
	can_message msg;
	msg.tx_header.StdId = 0x213;
	msg.tx_header.IDE = CAN_ID_STD;
	msg.tx_header.RTR = CAN_RTR_DATA;
	msg.tx_header.DLC = 8;
	msg.tx_header.TransmitGlobalTime = DISABLE;
	memcpy(msg.data, data, 8);
	xQueueSendToBack(hcap->can_queue, &msg, 100);
    return SUPER_CAP_OK;
#else
    CAN_TxHeaderTypeDef tx_header;
    uint32_t mailbox;
    tx_header.StdId = 0x213;
	tx_header.IDE = CAN_ID_STD;
	tx_header.RTR = CAN_RTR_DATA;
	tx_header.DLC = 8;
	tx_header.TransmitGlobalTime = DISABLE;
    if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) != 0)
        HAL_CAN_AddTxMessage(hcap->hcan, &tx_header, data, &mailbox);
    else
        hcap->status = SUPER_CAP_MAILFULL;
    return hcap->status;
#endif
}

void super_cap_callback(super_cap_TypeDef* hcap, CAN_RxHeaderTypeDef* rx_header, uint8_t* data)
{
    if (rx_header->StdId = 0x212)
    {
        hcap->bat_voltage = (float)((int16_t)(data[0] | (data[1] << 8))) / 100.0f;
        hcap->bat_current = (float)((int16_t)(data[2] | (data[3] << 8))) / 100.0f;
        hcap->cap_voltage = (float)((int16_t)(data[4] | (data[5] << 8))) / 100.0f;
        hcap->chassis_current = (float)((int16_t)(data[6] | (data[7] << 8))) / 100.0f;

        hcap->bat_power = hcap->bat_voltage * hcap->bat_current;
        hcap->chassis_power = hcap->bat_voltage * hcap->chassis_current;
        hcap->cap_power = hcap->chassis_power - hcap->bat_power;
    }
}
